#include <cassert>
#include <string>
#include <sstream>

#include "serialfile.h"
#include "convert.h"

#include <xmlrpc-c/base.hpp>
#include <xmlrpc-c/registry.hpp>
#include <xmlrpc-c/server_abyss.hpp>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread_time.hpp>
#include <boost/thread.hpp>

using namespace std;

//This enum stores the position of each piece of NavBox data
enum NBData
{
	state = 0,
	curHeading,
	tarHeading,
	curDepth,
	tarDepth,
	lDriveDC,
	rDriveDC,
	fDiveDC,
	rDiveDC,
	fLatDC,
	rLatDC,
	nbdataCount
};

string serialPort = "/dev/ttyUSB";
SerialFile* arduino;
string inBuffer = "";
string outBuffer = "";
vector<double> tempData;

//This array stores all of NavBox's data
double navboxData[nbdataCount];

//Class for the GetNavboxData XMLRPC "method"
class GetNavboxData : public xmlrpc_c::method
{
public:
    GetNavboxData() {}

    //The execute function is what runs when the XMLRPC "method" is called
    void execute(xmlrpc_c::paramList const& paramList,
				xmlrpc_c::value *  const  retvalP)
    {
    	vector<xmlrpc_c::value> tempVector;

    	for(int i = 0; i < nbdataCount; i++)
    		tempVector.push_back( xmlrpc_c::value_double(navboxData[i]) );

    	*retvalP = xmlrpc_c::value_array(tempVector);
    }
};

//Class for the SetTargetHeading XMLRPC "method"
class SetTargetHeading : public xmlrpc_c::method
{
public:
    SetTargetHeading() {}

    //The execute function is what runs when the XMLRPC "method" is called
    void execute(xmlrpc_c::paramList const& paramList,
				xmlrpc_c::value *  const  retvalP)
    {
    	navboxData[tarHeading] = paramList.getDouble(0);

    	outBuffer = "H ";
    	outBuffer += convertToString( navboxData[tarHeading] );
    	outBuffer += " Z ";

    	arduino->writeSerialData(outBuffer);

    	*retvalP = xmlrpc_c::value_double(navboxData[tarHeading]);
    }
};

//Class for the SetTargetDepth XMLRPC "method"
class SetTargetDepth : public xmlrpc_c::method
{
public:
    SetTargetDepth() {}

    //The execute function is what runs when the XMLRPC "method" is called
    void execute(xmlrpc_c::paramList const& paramList,
            xmlrpc_c::value *  const  retvalP)
    {
    	navboxData[tarDepth] = paramList.getDouble(0);

    	outBuffer = "D ";
    	outBuffer += convertToString( navboxData[tarDepth] );
    	outBuffer += " Z ";

    	arduino->writeSerialData(outBuffer);

    	*retvalP = xmlrpc_c::value_double(navboxData[tarDepth]);
    }
};

//Class for the SetDriveMotorDC XMLRPC "method"
class SetDriveMotorDC : public xmlrpc_c::method
{
public:
    SetDriveMotorDC() {}

    //The execute function is what runs when the XMLRPC "method" is called
    void execute(xmlrpc_c::paramList const& paramList,
            xmlrpc_c::value *  const  retvalP)
    {
    	navboxData[lDriveDC] = paramList.getDouble(0);
    	navboxData[rDriveDC] = paramList.getDouble(1);

    	outBuffer = "S ";
    	outBuffer += convertToString(navboxData[lDriveDC]);
    	outBuffer += " ";
    	outBuffer += convertToString(navboxData[rDriveDC]);
    	outBuffer += " Z ";

    	arduino->writeSerialData(outBuffer);

    	*retvalP = xmlrpc_c::value_int(0);
    }
};

//Class for the SetFrontLatMotorDC XMLRPC "method"
class SetFrontLatMotorDC : public xmlrpc_c::method
{
public:
    SetFrontLatMotorDC() {}

    //The execute function is what runs when the XMLRPC "method" is called
    void execute(xmlrpc_c::paramList const& paramList,
            xmlrpc_c::value *  const  retvalP)
    {
    	navboxData[fLatDC] = paramList.getDouble(0);

    	outBuffer = "F ";
    	outBuffer += convertToString(navboxData[fLatDC]);
    	outBuffer += " Z ";

    	arduino->writeSerialData(outBuffer);

    	*retvalP = xmlrpc_c::value_double(navboxData[fLatDC]);
    }
};

//Class for the SetRearLatMotorDC XMLRPC "method"
class SetRearLatMotorDC : public xmlrpc_c::method
{
public:
    SetRearLatMotorDC() {}

    //The execute function is what runs when the XMLRPC "method" is called
    void execute(xmlrpc_c::paramList const& paramList,
            xmlrpc_c::value *  const  retvalP)
    {
    	navboxData[fLatDC] = paramList.getInt(0);

    	outBuffer = "R ";
    	outBuffer += convertToString(navboxData[fLatDC]);
    	outBuffer += " Z ";

    	arduino->writeSerialData(outBuffer);

    	*retvalP = xmlrpc_c::value_int(navboxData[fLatDC]);
    }
};

void getSubDataFromArduino()
{
    double i;

	//Get NavBox data
    inBuffer = arduino->getSerialData();

    //Initialize the stringstream used to convert the data
    stringstream stream(inBuffer);

    //Convert each piece of data from string to double
    while(stream >> i)
    {
    	//Store each piece of data in the temporary storage vector
    	tempData.push_back(i);

    	//Ignore any commas
    	if(stream.peek() == ',')
    		stream.ignore();
    }

    //Assign each piece of data of the appropriate position in the data array,
    //as long as it fits the proper range
    try
    {
    	if(tempData[state] == 1 || tempData[state] == 0)
    		navboxData[state] = tempData[state];

    	if(tempData[curHeading] >= 0 && tempData[curHeading] < 360)
    		navboxData[curHeading] = tempData[curHeading];

    	if(tempData[tarHeading] >= 0 && tempData[tarHeading] < 360)
    		navboxData[tarHeading] = tempData[tarHeading];

    	if(tempData[curDepth] >= 0 && tempData[curDepth] <= 16)
    		navboxData[curDepth] = tempData[curDepth];

    	if(tempData[tarDepth] >= 0 && tempData[tarDepth] <= 16)
    		navboxData[tarDepth] = tempData[tarDepth];

    	if(tempData[lDriveDC] >= -100 && tempData[lDriveDC] <= 100)
    		navboxData[lDriveDC] = tempData[lDriveDC];

    	if(tempData[rDriveDC] >= -100 && tempData[rDriveDC] <= 100)
    		navboxData[rDriveDC] = tempData[rDriveDC];

    	if(tempData[fDiveDC] >= -100 && tempData[fDiveDC] <= 100)
    		navboxData[fDiveDC] = tempData[fDiveDC];

    	if(tempData[rDiveDC] >= -100 && tempData[rDiveDC] <= 100)
    		navboxData[rDiveDC] = tempData[rDiveDC];

    	if(tempData[fLatDC] >= -100 && tempData[fLatDC] <= 100)
    		navboxData[fLatDC] = tempData[fLatDC];

    	if(tempData[rLatDC] >= -100 && tempData[rLatDC] <= 100)
    		navboxData[rLatDC] = tempData[rLatDC];
    }
    catch(...)
    {
		cout << "What?\n";
    }

    //Clear the temporary storage vector
    tempData.clear();
}

int
main(int           const argc,
     const char ** const argv)
{
	//Handle command-line arguments, if any
	switch(argc)
	{
	case 2:
		serialPort += argv[1];
		break;
	default:
	    serialPort += "0";
		break;
	}

	//Initialize NavBox data array
    for(int i = 0; i < nbdataCount; i++)
    	navboxData[i] = 0;

    //Create XMLRPC server method registry
    xmlrpc_c::registry myRegistry;

    //Creates XMLRPC server "methods"
    xmlrpc_c::methodPtr const getNavboxDataMethod(new GetNavboxData);

    xmlrpc_c::methodPtr const setTargetHeadingMethod(new SetTargetHeading);
    xmlrpc_c::methodPtr const setTargetDepthMethod(new SetTargetDepth);
    xmlrpc_c::methodPtr const setDriveMotorDCMethod(new SetDriveMotorDC);
    xmlrpc_c::methodPtr const setFrontLatMotorDCMethod(new SetFrontLatMotorDC);
    xmlrpc_c::methodPtr const setRearLatMotorDCMethod(new SetRearLatMotorDC);

    //Adds XMLRPC methods to the server
    myRegistry.addMethod("sdcr.GetNavboxData", getNavboxDataMethod);

    myRegistry.addMethod("sdcr.SetTargetHeading", setTargetHeadingMethod);
    myRegistry.addMethod("sdcr.SetTargetDepth", setTargetDepthMethod);
    myRegistry.addMethod("sdcr.SetDriveMotorDC", setDriveMotorDCMethod);
    myRegistry.addMethod("sdcr.SetFrontLatMotorDC", setFrontLatMotorDCMethod);
    myRegistry.addMethod("sdcr.SetRearLatMotorDC", setRearLatMotorDCMethod);

    //Create XMLRPC server
    xmlrpc_c::serverAbyss myAbyssServer(xmlrpc_c::serverAbyss::constrOpt()
			.registryP(&myRegistry)			 // Method registry
			.portNumber(8087)                // TCP port on which to listen
			.logFileName("/tmp/cmdr_serial_xmlrpc_log")  // Log file
        );


    //Initialize USB connection to Arduino
    arduino = new SerialFile(serialPort, SerialPort::BAUD_115200);

    // Wait for serial port (NavBox) to initialize
    boost::this_thread::sleep( boost::posix_time::milliseconds( 2000 ) );

    //Main loop
    while(true)
    {
    	//Get NavBox data
        getSubDataFromArduino();

        //Run the XMLRPC server
    	myAbyssServer.runOnce();
    }

    return 0;
}
